00 

o 

O 
(N 

u 

Ph. 
< 



in 
U 



Non-linear Symmetry-preserving Observers on 

Lie Groups 

Silvere Bonnabel, Philippe Martin and Pierre Rouchon 



Abstract 



In this paper we give a geometrical framework for the design of observers on finite-dimensional 
Lie groups for systems which possess some specific symmetries. The design and the error (between true 
, and estimated state) equation are explicit and intrinsic. We consider also a particular case: left-invariant 

■ systems on Lie groups with right equivariant output. The theory yields a class of observers such that 

error equation is autonomous. The observers converge locally around any trajectory, and the global 
behavior is independent from the trajectory, which reminds the linear stationary case. 



(N 

> , 

^ ! I. Introduction 

oo 

^ . Symmetries (invariances) have been used to design controllers and for optimal control theory 

^ ! ([6], [9], [7], [15], [12], [13]), but far less for the design of observers. [4] develops a theory 
^ ■ of symmetry-preserving observers and presents three non-hnear observers for three examples of 
engineering interest: a chemical reactor, a non-holonomic car, and an inertial navigation system. 
^ ' In the two latter examples the state space and the group of symmetry have the same dimension 
and (since the action is free) the state space can be identified with the group (up to some discrete 
group). Applying the general theory to the Lie group case, we develop here a proper theory of 
symmetry-preserving observers on Lie groups. The advantage over [4] is that the observer design 
is explicit (the implicit function theorem is not needed) and intrinsic, the error equation and its 
first-order approximation can be computed explicitly, and are intrinsic, and all the formulas are 
globally defined. Moreover, this paper is a step further in the symmetry-preserving observers 
theory since [4] does not deal at all with convergence issues in the general case. Here using 
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the explicit error equation we introduce a new class of trajectories around which we build 
convergent observers. In the case of section |lll] a class of first-order convergent observers around 
any trajectory is given. The theory applies to various systems of engineering interest modeled as 
invariant systems on Lie groups, such as cart-like vehicles and rigid bodies in space. In particular 
it is well suited to attitude estimation and some inertial navigation examples. 

The paper is organized as follows: in section |Il] we give a general framework for symmetry- 
preserving observers on Lie groups. It explains the general form of the observers [10], [8], 
[5] and [4] based on the group structure of S0(3) and (resp.) SE(2), without considering the 
convergence issues. The design, the error equation and its first-order approximation are given 
explicitly. It is theoretically explained why the error equation in the car example of [4] does not 
depend on the trajectory (although it depends on the inputs). Then we introduce a new class of 
trajectories called permanent trajectories which extend the notion of equilibrium point for systems 
with symmetries: making a symmetry-preserving observer around such a trajectory boils down to 
make a Luenberger observer around an equilibrium point. We characterize permanent trajectories 
geometrically and give a locally convergent observer around any permanent trajectory. 

In section Ull] we consider the special case of a left-invariant system with right equivariant 
output. It can be looked at as the motion of a generalized rigid body in space with measurements 
expressed in the body-fixed frame, as it will be explained in section IIII-A.1[ Thus it applies to 
some inertial navigation examples. In particular it allows to explain theoretically why the error 
equation in the inertial navigation example of [4] is autonomous. A class of first-order convergent 
observers such that the error equation is autonomous is derived. This property reminds much of 
the linear stationary case. We also explore the links between right equivariance of the output 
map and observability. 

Preliminary versions of section |lll] can be found in [2], [3]. 



II. Symmetry-preserving observers on Lie groups 



A. Invariant observer and error equation 



Consider the following system : 



d 
di 



(1) 



y = h{x, u) 



(2) 
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where x G G, u G U = M™, y G y = W (the whole theory can be easily adapted to the case 
where 14 and y are smooth m and p-dimensional manifolds, for instance Lie groups), and / is 
a smooth vector field on G. u E U is a known input (control, measured perturbation, constant 
parameter, time t etc.). 

Definition 1: Let G be a Lie Group with identity e and S an open set (or more generally a 
manifold). A left group action {(pg)geG on S is a smooth map 

such that: 

. = e for all e 

• KiKiO) = KgiiO for all gi,g2,C 
In analogy one defines a right group action the same way except that (09i(O) = 09i92(O for 
all 91,92, Suppose G acts on the left on U and y yisl ipg : U ^ U and Pg : y ^ y. Suppose 
the dynamics © is invariant in the sense of [4] where the group action on the state space (the 
group itself) is made of left multiplication: for any g E G, DLgf(x,u) = f{gx^'iljg{u)), i.e: 

yx,9EG f{Lg{x),iljg{u)) = DLgf{x,u) 

where Lg : x i-^ gx is the left multiplication on G, and DLg the induced map on the tangent 
space. DLg maps the tangent space TG\x to TG\gx. Let Rg : x ^ xg denote the right 
multiplication and DRg its induced map on the tangent space. As in [4], we suppose that the 
output y = h{x,u) is equivariant, i.e, h(^(pg{x),iljg{u)) = Pg{h{x,u)) for all g,x,u. 

Definition 2: Consider the change of variables X = gx, U = ^g{u) and Y = Pg{y). The 
system ©-© is left-invariant with equivariant output if for all g E G it is unaffected by the 
latter transformation: |X(t) = f{X, U), Y = h{X, U). 

We are going to build observers which respect the symmetries (left-invariance under the group 
action) adapting the constructive method of [4] to the Lie group case. 

1) Invariant pre-observers: Following [14] (or [4]) consider the action {(pg)geG of G on S = 
W where s is any positive integer. Let (x, z) E GxW, one can compute (at most) s functionally 
independent scalar invariants of the variables (x, z) the following way: I(x, z) = 0^.-1 (z) E M'^ . 
It has the property that any invariant real-valued function J(x, z) which verifies J{gx, (f)g{z)) = 
J{x, z) for all g, x,z is a function of the components of I{x, z): J(x, z) = 'H{I{x, z)). Applying 
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this general method we find a complete set of invariants of (x, u) E G x U: 



I{x, u) = i/j^-i (u) G U. 



(3) 



Take n linearly independent vectors (Wi, . . . , VF„) in TG\e = g, the Lie algebra of the group 
G. Define n vector fields by the invariance relation Wi{x) = DL^Wi E TG\x,i = l...n,x E G. 
The vector fields form an invariant frame [14]. According to [4] 

Definition 3 (pre-observer): The system -^x = F{x, u, y) is a pre-observer of ([I])-© if 



The definition does not deal with convergence; if moreover x{t)~^x{t) — e as t — > +oo for 
every (close) initial conditions, the pre-observer is an (asymptotic) observer. It is said to be is 
G-invariant if F(^gx, ipg{u), gg{y)) = DLgF{x, u, y) for all {g,x,u,y) E G x G x U x y. 
Lemma 1: Any invariant pre-observer reads 



where the Ci are any smooth functions of their arguments such that £j (i/j^-i^u), h{e, ip^-i^u))) = 
0. The proof is analogous to [4]: one can write DL£-i{^x — f{x, u)) = XliLi -^^ '^^ v) ^« ^ S' 
where the jF^'s are invariant scalar functions of their arguments. But a complete set of invariants of 
x, u, y is made of the components of {i>j.-i{u), ps:-i{y)), thus jFj (x, u, y) = Ci {i/j£-i{u), pj.-i{y)). 
And when x = x we have Px-^iy) = h{x^^x,i/j£-i{u)) = h{e,ijjx-^{u)) and the £j's cancel. 

2) Invariant state-error dynamics: Consider the invariant state-error i] = x~^x E G. It 
is invariant by left multiplication : ij = {gx)^^{gx) for any g E G. Notice that a small error 
corresponds to rj close to e. Contrarily to [4], the time derivative of rj can be computed explicitly. 
We recall Rg denotes the right multiplication map on G. Since we have 

. for any gi,g2E G, DLg^DLg^ = DLg^g^, DRg^DRg^ = DRg^g^, DLg^DRg^ = DRg^DLg^ 

. I{x,u) = ipx-iiu) = ^lj(xv)-iiu) 

. p£-i(/i(x,ii)) = /i(x-^x,^£-i(m)) writes p£-i{y) = /i(?7"\ (m)) 
• iv = = DL^^iix - DR^f^x-' with |x-i = -DL,-rDR,-ij^x 

the error dynamics reads 



F[x, u, h{x, u 



')) = f{.x, u) for all (x, u) E G xU. 




(4) 



+ DL^ {^ix^)-i{u),h{r] \^/'(^.^)-i(u))) Wi . (5) 




1=1 
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The invariant error rj obeys a differential equation that is coupled to the system trajectory t 
{x{t),u{t)) only via the invariant term I(x,u) = ^p^-i^u). Note that when i'g{u) = u the 
invariant error dynamics is independent of the state trajectory x{t)\ This the reason why we 
have this property in the non-holonomic car example of [4]. 

3) Invariant first order approximation: For r] close to e, one can set in ([5]) 77 = exp(e^) where 
is an element of the Lie algebra q and e G M is small. The linearized invariant state error 
equation can always be written in the same tangent space g: up to order second terms in e 

d df Oip 

= [C,f{e,i^x-i{u))] - -^{e,i}^-i{u)) — {e,i)^-i{u))i 

" f dC- dh \ 

- ( ^(V^^-iM'^(e,^x-i(M))^(e,V^x'i(M))n Wi (6) 

i=l ^ ' 



where [,] denotes the Lie bracket of g, ip is viewed as a function of {g,u), and ^ denotes the 
partial derivative of Ci with respect to its second argument. The gains ^{ip^-^iu), h{e, ^px-l{u)) 
can be tuned via linear techniques to achieve local convergence. 



dh 

I 1/1 1 I 1 I 

dh 



B. Local convergence around permanent trajectories 

The aim of this paragraph is to extend local convergence results around an equilibrium point 
to a class of trajectories we call permanent trajectories. 

Definition 4: A trajectory of ([I]) is permanent if I{x{t), u(t)) = / is independent of t. 
Note that adapting this definition to the general case of symmetry-preserving observers [4] is 
straightforward. Any trajectory of the system verifies ■^x{t) = DLx{t)f{e, ipj.(^t)~^{u{t))) thanks to 
the invariance of the dynamics. It is permanent if I{x{t), u{t)) = = uis, independent 

of t. The permanent trajectory x{t) is then given by x(0) exp(ttf;) where w is the left invariant 
vector field associated to f{e,u). Thus x{t) corresponds, up to a left translation defined by the 
initial condition, to a one-parameter sub-group. 

Let us make an observer around an arbitrary permanent trajectory: denote by {xr{t),Ur{t)) 
a permanent trajectory associated to m = tp--i^^-jUr{t). Let us suppose we made an invariant 
observer following (|4]). Then the error equation ([5]) writes 

V = DL,f{e,^,-i{u))-DRJ{e,u) + DL, ( ^ A ( (m), (^))) W^. ) • (V) 



dt 



i=l 
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since (^) = = V^jy-^l^)- The first order approximation Q is now a time 

invariant system: 

Let us write ^ and /(e, w) in the frame defined by the Wi's: ^ = J2k=i ^'^^fc ^'^^ /(^' ^) = 
Sfc=i f^^k - Denote by Cfj the structure constants associated with the Lie algebra of G: \Wi, Wj] = 
X]fc=i CijWk- The above system reads: 

d 



^^^={A + CC)^ (8) 



where 



l<i,j;'<n 



1 < fe < p 



l<fe<p l<j<n 

where are the local coordinates around e defined by the exponential map: x = 

exp(^"^^ XjW^j). If we assume that the pair {A,C) is observable we can choose the poles of 
A + CC to get an invariant and locally convergent observer around any permanent trajectory 
associated to u. Let W{x) = \Wi{x), .., It suffices to take: 

j^x = fix, u{t)) + W{x)Cp,-. (yit)) (9) 

Examples: In the non-holonomic car example of [4], permanent trajectories are made of 
lines and circle with constant speed. In the inertial navigation example of [4], ipx-i^u) = 

, a trajectory is permanent if q * uj * q and q*{a + vXLo)*q 

q* {a + V ^ uj) * q~^ j 
are independent of t. Some computations show that any permanent trajectory reads: 

q{t) = exp [^t] * qo 



'^i't) = Qo^ * (^{Xilt + T + exp (^"'^^ * r * exp ^^^^^ * 

where Vl, T and T are constant vectors of R^, A is a constant scalar and qo is a unit-norm 
quaternion. Theses constants can be arbitrarily chosen. Hence, the general permanent trajectory 
corresponds, up to a Galilean transformation, to an helicoidal motion uniformly accelerated along 



April 5, 2008 



DRAFT 



7 



the rotation axis when A 7^ 0; when A tends to infinity and to 0, we recover as a degenerate 
case a uniformly accelerated line. When A = and F = we recover a coordinated turn. 

III. Left invariant dynamics and right equivariant output 
A. Invariant observer and error equation 

1 ) Left invariant dynamics and right equivariant output: Consider the following system: 

^x(t) = /(x,t) (10) 

y = h{x) (11) 

where we still have x G G, y E y, and / is a smooth vector field on G. Let us suppose 
the dynamics (fTOl) is /e//-invariant (see e.g [1]), i.e: V(7,x G G f{Lg{x),t) = DLgf{x,t). 
For all g E G, the transformation X{t) = gx{t) leaves the dynamics equations unchanged: 
^X(t) = f(X(t),t). As in [1] let Ug = DL^-i-^x E g. Indeed one can look at any left invariant 
dynamics on G as a motion of a "generalized rigid body" with configuration space G. Thus one 
can look at LOs{t) = f{e,t) as the "angular velocity in the body", where e is the group identity 
element (whereas DR^-i^x is the "angular velocity in space"). We will systematically write 
the left-invariant dynamics (fTOl) 

^x{t) = DL,uo,{t) (12) 
at 

Let us suppose that /i : G ^ F is a right equivariant smooth output map. The group action 
on itself by right multiplication corresponds to the transformations {pg)g<=G on the output space 
y-. for all x,g E G, h{xg) = pg{h{x)) i.e 

Left multiplication corresponds then for the generalized body to a change of space-fixed frame, 
and right multiplication to a change of body-fixed frame. If all the measurements correspond to 
a part of the state x expressed in the body-fixed frame, they are affected by a change of body- 
fixed frame, and the output map is right equivariant. Thus the theory allows to build non-linear 
observers such that the error equation is autonomous, in particular for cart-like vehicles and 
rigid bodies in space (according to the Eulerian motion) with measurements in the body-fixed 
frame (see the example below). 
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2) Observability: If the dimension of the output space is strictly smaller than the dimension 
of the state space (dim y < dim g) the system is necessarily not observable. This comes from the 
fact that, in this case, there exists two distinct elements xi and X2 of G such that h{xi) = h{x2). If 
x{t) is a trajectory of the system, we have -§j.x{t) = DLgUJs{t) and because of the left-invariance, 
gix{t) and g2x{t) are also trajectories of the system: 



But since h is right equivariant: h{gix{t)) = Px(t)h{gi) = Px(t)h{g2) = h{g2x(t)). The trajectories 
gix{t) and g2x{t) are distinct and for all t they correspond to the same output. The system is 
unobservable. 

3) Applying the general theory of section ^ There are two ways to apply the theory of 
section |IIl i) The most natural (respecting left-invariance) does not yield the most interesting 
properties: let W = M x 3^ and let us look at (111,^2) = {t,h{e)) as inputs. For all g E G let 
il)g{t,h{e)) = {t, pg-i{h{e))). Define a new output map H{x,u) = h{x) = px{h{e)) = Px{u2)- It 
is unchanged by the transformation introduced in definition [21 since H{X, U) = Pgx{pg-^{u2)) = 
H(x, u) for all g E G. (fTOl) -(fTTI) is then a left-invariant system in the sense of definition [2l when 
the output map is H(x, u). ii) Let us rather look at ujs{t) as an input : u{t) = cOsii) £ where 
W = g = is the input space. Let us define for all g the map ipg : G ^ U the following way 



It means ipg is the differential of the interior automorphism of G. And the dynamics (flOl) writes 
■j^x = F{x,u) = DLxU and can be viewed as a right-invariant dynamics. For all x,g we have 
indeed: 



-i?g(x) = DRgDLxUJsit) = DLxDLgDLg-iDRgUsit) = DLR^(^x)M^s{t)) = F{Rg{x),^g{u)) 
{'^g)geG and {pg)geG are right group actions since for all gi, g2 ^ G we have ifjg^ o i/jg^ = ipg^g^ 



and o pg^ = pg^g^. Thus we strictly apply the general theory of III exchanging the roles of 
left and right multiplication. 

4) Construction of the observers: Take n linearly independent vectors {Wi, . . . , Wn) in TG\e = 
Q. Consider the class of observers of the form 



dt 



{gix{t)) = DLg^x^^sif) 



d 
'dt 



{g2x{t)) = DLg^x^^sif)- 



iPg = DLg-lDRg 



d 




(13) 



i=l 
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where the £j's are smooth scalar functions such that Ci{h{e)) = 0. They are invariant under the 
transformations defined above in section IIII-A.3l -ii). 

5) State-error dynamics: The error (invariant by right multiplication) is G 3 7] = (xx^^) = 
Lxix~^). The error equation is an autonomous differential equation (O independent from the 
trajectory t ^-^ x{t) (as in the linear stationary case): 

-V = DRr,{J2^^iHv-'))Wi) (14) 

i=l 

It can be deduced from ^ or directly computed using ^r] = DL^^^x^^) + DxL^{x^^)-^x and 
. DxL^{x~')ix = DR,-.{ix) = DR^-.DL^ u^it) + DR,-^DRs,Y.l=l^^^{Px-Ay)W^ = 

DR^-iDL^ uJs{t) + DR^YJU C-^{ps^-Ay)W^ 
. DLs^if^x-^) = -DL^DR^-iDL^-ix = -DLiDR^-iUs = -DR^-iDLs^cOsit) 
• A(P£-i(l/)) = Ci{p£-i{h{x))) = Ci{h{r]-^)). 

6) First order approximation: We suppose that r] is close to e. Let ^ G g such that r] = exp(e^) 
with e G M small. We have up to second order terms in e 

i=l ^ ' 

Let us define a scalar product on the tangent space g at e, and let us consider the adjoint 
operator of Dh{e) in the sense of the metrics associated to the scalar product. The adjoint 
operator is denoted by {Dh{e)Y and we take C{y) = K(Dh{e))^{y — h{e)). The first order 
approximation writes 

^ = -K Dh^ Dh i (15) 

and for K > 0, admits as Lyapunov function which the length of ^ in the sense of the 
scalar product. 

B. A class of non-linear first-order convergent observers 

Consider for ([IOl)-([n]) the following observers : |x = DLiUJs{t)+DRi[Y,'l^^[C.,{p'^^{h{x)))]Wi] 
where the £j's are smooth scalar functions such that Ci{h{e)) = 0. Using the first order 
approximation design, take such that the symmetric part (in the sense of the scalar 

product chosen on TG\e) of the linear map C ^ - Yh=i (^(^(^))f^(^)0 negative. When 
it is negative definite, we get locally exponentially convergent non-linear observers around any 
system trajectory. 
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IV. Brief example: Magnetic-aided attitude estimation 

To illustrate briefly the theory we give one of the simplest example: magnetic-aided inertial 
navigation as considered in [11], [3]. We just give the system equations, the application of the 
theory to this example being straightforward. It is necessary in order to pilot a flying body to 
have at least a good knowledge of its orientation. This holds for manual, or semi automatic or 
automatic piloting. In low-cost or "strap-down" navigation systems the measurements of angular 
velocity uj and acceleration a by rather cheap gyrometers and accelerometers are completed by 
a measure of the earth magnetic field B. These various measurements are fused (data fusion) 
according to the motion equations of the system. The estimation of the orientation is generally 
performed by an extended Kalman filter. But the use of extended Kalman filter requires much 
calculus capacity because of the matrix inversions. The orientation (attitude) can be described 
by an element of the group of rotations S0(3), which is the configuration space of a body fixed 
at a point. The motion equation are 

jR = R{u X ■) (16) 

where 

» R E S0(3) is the quaternion of norm one which represents the rotation which maps the 

body frame to the earth frame, 
• a;(t) is the instantaneous angular velocity vector measured by gyroscopes and (a; x ■) the 
skew- symmetric matrix corresponding to wedge product with u. 
If the output is the earth magnetic field B measured by the magnetometers in the body-fixed 
frame y = R'^B ([5]), the output is right equivariant. The output has dimension 2 (the norm of 
y is constant) and the state space has dimension 3. Thus the system is not observable according 
to section iriI-A.2[ This is why we make an additional assumption as in [11], [3]. Indeed the 
accelerometers measure a = 4:^ + R^^G where 4v is the acceleration of the center of mass of 

at at 

the body and G is the gravity vector. We suppose the acceleration of the center of mass is small 
with respect to || G \\ (quasi- stationary flight). The measured output is thus y = {yc, ys) = 
(R^^G, R^^B). One can apply the theory as described in section llII-A.3l -i) or IIII-A.3l -ii). 
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V. Conclusion 

In this paper we completed the theory of [4] giving a general framework to symmetry- 
preserving observers when the state space is a Lie group. The observers are intrinsically and 
globally defined. By the way, we explained the nice properties of the error equation in two 
examples of [4]. In particular we derived observers which converge around any trajectory and 
such that the global behavior is independent of the trajectory as well as of the time-varying 
inputs for a general class of systems. 
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